#include "smash_video_server.h"

#include "vxWorks.h" 
#include "taskLib.h" 
#include "socket.h" 
#include "netinet/in.h" 
#include "stdio.h" 
#include "stdlib.h" 
#include "string.h" 
#include "sockLib.h" 
#include "inetLib.h" 
#include "ioLib.h" 
#include "routeLib.h" 

#include <iostream>
#include <fstream>
#include <sstream>
#include <string.h>

#include "WPILib.h"
#include "smash_camera.h"

namespace {

static const int kPacketSize(1032);
static const int kHeaderSize(sizeof(short) * 4);
static const int kJpegSize(kPacketSize - kHeaderSize);

typedef struct VideoPacket {
    unsigned short index;
    unsigned short count;
    unsigned short handle;
    unsigned short size;
    char image[kJpegSize];
    
    VideoPacket(): index(0),
                   count(0),
                   handle(0) {
        memset(image, 0, sizeof(image));
    }
};

} // namespace

namespace smash {

PCVideoServer::PCVideoServer():
  server_task_("MagicPCVideoServer", (FUNCPTR)VideoServerHelper) {
	FILE* F = fopen("smash_ip", "r");
	if(F != NULL) {
		char ip[16];
		if(fgets(ip, 16, F)) {
			PCVideoIP = ip;
		}
	}
	server_task_.Start();
}

PCVideoServer::~PCVideoServer() {
    server_task_.Stop();     
}

void PCVideoServer::Start() {
    server_task_.Start();
}

void PCVideoServer::Stop() {
    server_task_.Stop();
}

void PCVideoServer::VideoServerHelper() {
    char* receiver_ip = const_cast<char*>(PCVideoDefaultIP);
    if(PCVideoIP) {
    	std::cout << "PCVideoIP not set\n";
        receiver_ip = PCVideoIP;
    }
    std::cout << "ip " << receiver_ip << " : " << PCVideoPort << std::endl;
    int sock(socket(AF_INET, SOCK_DGRAM, 0));    
    if(sock < 0) {
        std::cout << "Failed to create socket"
                  << std::endl;
        return;
    }
    sockaddr_in sendaddr;
    memset(&sendaddr, 0, sizeof(sendaddr));
    sendaddr.sin_len = sizeof(sendaddr);
    sendaddr.sin_family = AF_INET;
    sendaddr.sin_addr.s_addr = inet_addr(receiver_ip);
    if(sendaddr.sin_addr.s_addr
         == static_cast<unsigned>(ERROR)) {
    	sendaddr.sin_addr.s_addr
    	  = inet_addr(const_cast<char*>(PCVideoDefaultIP));
        if(sendaddr.sin_addr.s_addr
             == static_cast<unsigned>(ERROR)) {
    		std::cout << "IP Addresses are bad";
    		return;
    	}
    }
    sendaddr.sin_port = htons(PCVideoPort);
    VideoPacket packet;
    char* image_data(0);
    int image_length(0);
    for(;;) {
        if(AxisCamera::getInstance().GetJpegImageBlocking(&image_data, &image_length)) {
            unsigned short packets_required = image_length / kJpegSize;
            if(image_length % kJpegSize > 0) {
                ++packets_required;
            }
            packet.index = 0;
            packet.count = packets_required;
            packet.size = kJpegSize;
            int video_data_sent(0);
            for(unsigned short i = 0; i < packets_required; ++i) {
                if(image_length - video_data_sent < kJpegSize) {
                    packet.size = image_length - video_data_sent;
                }
                packet.index = i;
                memcpy(packet.image, image_data, packet.size);
                image_data += packet.size;
                int send_len = sizeof(packet);
                char* buffer = reinterpret_cast<char*>(&packet);
                while(send_len > 0) {
                    int sent = sendto(sock, 
                               buffer,
                               send_len,
                               0,
                               reinterpret_cast<sockaddr*>(&sendaddr),
                               sizeof(sendaddr));
                    send_len -= sent;
                    buffer += sent;
                }
            }
            delete[] image_data;
            ++packet.handle;
        }
       Wait(.02);
    }
}

} // namespace magic

